// Copyright(c) 2018-2023 Hongjing
// All rights reserved.
//
// Author: Cheng Wang
// Update: 2018-07-03
#ifndef CIRCULAR_QUEUE_H_
#define CIRCULAR_QUEUE_H_

#include <array>
#include <cstdint>
#include <exception>

namespace autodrive {
namespace perception {
// Circular queue
template <class T, std::uint32_t size>
class CircularQueue {
 public:
  CircularQueue() noexcept : latest_index_{0U}, num_filled_{0U} {}
  ~CircularQueue() noexcept = default;
  CircularQueue(CircularQueue const&) = delete;
  CircularQueue& operator=(CircularQueue const&) = delete;
  CircularQueue(CircularQueue&&) = delete;
  CircularQueue& operator=(CircularQueue&&) = delete;

  /// @brief Get size
  ///
  /// @return size_
  std::uint32_t GetSize() const noexcept { return queue_.size(); }

  /// @brief Get filled count
  ///
  /// @return num_filled_
  std::uint32_t GetNumFilled() const noexcept { return num_filled_; }

  /// @brief Get latest object
  ///
  /// @throw ResourceNotExistException  when no object is available
  ///
  /// @return <latest index, latest object>
  std::pair<std::uint32_t, T*> GetLatest() noexcept(false) {
    if (num_filled_ == 0) {
      throw std::runtime_error("No object available in the queue");
    }
    return std::pair<std::uint32_t, T*>(latest_index_,
                                        &(queue_[latest_index_]));
  }

  /// @brief Get object at index
  ///
  /// @param index  object index
  /// @throw ResourceNotExistException  when no object is available
  ///
  /// @return object at index
  T* Get(std::uint32_t index) noexcept(false) {
    if (num_filled_ <= index) {
      throw std::runtime_error("Object not exist");
    }
    return &(queue_[index]);
  }

  /// @brief Get next object to append
  ///
  /// @throw ResourceNotExistException  when queue size is 0
  ///
  /// @return next object to append
  T* GetNext() noexcept(false) {
    if (queue_.size() == 0) {
      throw std::runtime_error("Invalid queue");
    }
    std::uint32_t index{0};
    if (num_filled_ > 0) {
      index = (latest_index_ + 1) % queue_.size();
    }
    return &(queue_[index]);
  }

  /// @brief Get next index to index
  ///
  /// @throw ResourceNotExistException  when queue size is 0
  /// @throw InvalidArgumentException  when index not in [0, num_filled - 1]
  ///
  /// @return next index to index
  std::uint32_t GetNextIndex(std::uint32_t index) const noexcept(false) {
    if (queue_.size() == 0) {
      throw std::runtime_error("Invalid queue");
    }
    if (index >= num_filled_) {
      throw std::runtime_error("Invalid queue index");
    }
    return (index + 1) % queue_.size();
  }

  /// @brief Get previous index to index
  ///
  /// @throw ResourceNotExistException  when queue size is 0 or prec indev
  /// @throw InvalidArgumentException  when index not in [0, num_filled - 1]
  ///
  /// @return previous index to index, queue size when reach the beginning
  std::uint32_t GetPrevIndex(std::uint32_t index) const noexcept(false) {
    if (queue_.size() == 0) {
      throw std::runtime_error("Invalid queue");
    }
    if (index >= num_filled_) {
      throw std::runtime_error("Invalid queue index");
    }
    if ((index == 0 && num_filled_ < queue_.size()) ||
        (latest_index_ + 1) % queue_.size() == index) {
      throw std::runtime_error("No previous index");
    }
    return (index + queue_.size() - 1) % queue_.size();
  }

  /// @brief Check if an index has previous index
  ///
  /// @throw ResourceNotExistException  when queue size is 0
  /// @throw invalid_argument  when index not in [0, num_filled - 1]
  ///
  /// @return true if previous index exists, otherwise false
  bool HasPrev(std::uint32_t index) const noexcept(false) {
    if (queue_.size() == 0) {
      throw std::runtime_error("Invalid queue");
    }
    if (index >= num_filled_) {
      throw std::runtime_error("Invalid queue index");
    }
    return (index > 0 || num_filled_ == queue_.size()) &&
           (latest_index_ + 1) % queue_.size() != index;
  }

  /// @brief Get the index of an item
  ///
  /// @throw InvalidArgumentException  when ptr not in the queue
  ///
  /// @return index
  std::uint32_t GetIndex(T* ptr) const noexcept(false) {
    std::int64_t index{ptr - queue_.data()};
    if (index < 0 || index >= static_cast<std::int32_t>(queue_.size())) {
      throw std::runtime_error("Ptr not in range");
    }
    return static_cast<std::uint32_t>(index);
  }

  /// @brief Append an object
  ///
  /// @param object  object to append
  /// @throw ResourceNotExistException  when queue size is 0
  void Append(T const& object) noexcept(false) {
    if (queue_.size() == 0U) {
      throw std::runtime_error("Invalid queue");
    }
    std::uint32_t index{0U};
    if (num_filled_ > 0) {
      index = (latest_index_ + 1) % queue_.size();
    }
    queue_[index] = object;

    latest_index_ = index;
    if (num_filled_ < queue_.size()) {
      num_filled_++;
    }
  }

  /// @brief Append an object
  ///
  /// @param object  object to append
  /// @throw ResourceNotExistException  when queue size is 0
  void Append(T&& object) noexcept(false) {
    if (queue_.size() == 0) {
      throw std::runtime_error("Invalid queue");
    }
    std::uint32_t index{0U};
    if (num_filled_ > 0) {
      index = (latest_index_ + 1) % queue_.size();
    }
    queue_[index] = object;

    latest_index_ = index;
    if (num_filled_ < queue_.size()) {
      num_filled_++;
    }
  }

  /// @brief Append the next object
  ///
  /// @throw ResourceNotExistException  when queue size is 0
  void Append() noexcept(false) {
    if (queue_.size() == 0) {
      throw std::runtime_error("Invalid queue");
    }
    if (num_filled_ > 0) {
      latest_index_ = (latest_index_ + 1) % queue_.size();
    } else {
      latest_index_ = 0;
    }
    if (num_filled_ < queue_.size()) {
      num_filled_++;
    }
  }

  /// @brief ACLear objects
  ///
  /// @throw ResourceNotExistException  when queue size is 0
  void Clear() noexcept { num_filled_ = 0U; }

 private:
  /// @brief Queue
  std::array<T, size> queue_;
  /// @brief Latest index
  std::uint32_t latest_index_;
  /// @brief Number of objects filled
  std::uint32_t num_filled_;
};

}  // namespace perception
}  // namespace autodrive

#endif  // CIRCULAR_QUEUE_H_

/* vim: set ts=2 sw=2 sts=2 tw=80 et: */
